1. Field
Embodiments of the present invention relate to estimating vehicle trajectories, specifically optimizing the pose of a vehicle to reduce drift.
2. Related Art
Laser range-finders have become standard equipment in mobile robotics, where they are used for simultaneous localization and mapping (SLAM) tasks, both for computing a local robot trajectory (i.e., the incremental movement of the robot and the environment geometry) and for estimating loop-closing constraints.
Scan matching is typically performed between pairs of scans, using variants of the original iterative closest point (ICP) method, which aligns scan points to scan points or scan points to line segments. Scan point to scan point alignment is further described in, for example, F. Lu and E. Milios, “Robot pose estimation in unknown environments by matching 2d range scans,” Journal of Intelligent and Robotic Systems, 18:249-275, 1997, which is incorporated by reference herein in its entirety. Scan point to line segment alignment is further described in, for example, I. Cox, “Blanche: an experiment in guidance and navigation of an autonomous robot vehicle,” IEEE Trans. on Robotics and Automation, 7(2): 193-204, 1991, which is incorporated by reference herein in its entirety. However, these approaches have had mixed success due to the nature of the laser data. Standard lasers can often have poor resolution (on the order of 1 degree per laser reading), which causes distant surfaces to be sparsely sampled. The laser scans also contain outliers and noise, due to surface reflectance and to moving objects in the environment. Because it is difficult to handle these issues robustly using pairs of scans at a time, standard scan matching algorithms can produce errors that cause the estimated pose to deviate significantly from the true trajectory. In the absence of absolute pose information in the form of GPS or building maps, these errors can grow arbitrarily large.
A less popular alternative to ICP has been to replace both scans using histograms and solve the matching using the maximum of a cross correlation function. Such an approach is further described in, for example, G. Weiss and E. Puttamer, “A map based on laserscans without geometric interpretation,” Intelligent Autonomous Systems, pp. 403-407, 1995, which section is incorporated herein by reference. It has been known that aligning groups of scans to each other can produce more accurate results, but because of the computational complexity associated with a quadratic number of scan pairs, only a very small number of scans can be handled efficiently in this manner.
A very popular class of SLAM methods, based on occupancy grid maps, have used an occupancy grid map representation to align scans. SLAM based on occupancy grid maps is further described in, for example, S. Thrun, D. Fox, and W. Burgard, “A probabilistic approach to concurrent mapping and localization for mobile robots,” Machine Learning, 31, 1998, which is incorporated by reference herein in its entirety. Occupancy grid maps represent environments by fine-grained, metric grids of binary variables that encode whether the corresponding cells are free or occupied. Occupancy grids make the definition of Monte Carlo sampling algorithms fairly straightforward and many such algorithms have been developed. Monte Carlo algorithms maintain multiple discrete alignment hypotheses using particles to increase the robustness of the SLAM algorithms with regards to matching errors (Monte Carlo methods have also been successfully used in conjunction with scan matching, but at the cost of increased implementation complexity). Despite the success of these methods in preventing large alignment errors, the occupancy grid map representation is inherently discrete and does not support the estimation of local gradients, which prevents the robot pose from being determined locally with high precision. As a result, a large number of particles and diverse sampling schemes need to be employed to ensure that the correct trajectory has been sampled. The locally imprecise poses can accumulate to cause large drift over time.
Additional alternative approaches to reduce drift errors include utilizing a known map of the environment and manual pose corrections by an operator. Each of these requires additional data, which may not be available.